#include <esp_now.h>
#include <WiFi.h>
#include <U8g2lib.h>
#include <Wire.h>

// 定义摇杆及LED引脚
const int LEFT_X_PIN = 4;     // 左X
const int LEFT_Y_PIN = 5;     // 左Y
const int RIGHT_X_PIN = 6;    // 右X
const int RIGHT_Y_PIN = 7;    // 左Y
const int LED_PIN = 1;        //LED

//摇杆死区
int deadZone = 1000;

// ADC引脚及参数
const int ADC_PIN = 8;             // ADC引脚
const float R1 = 10000.0;          // 分压电阻R1=10kΩ
const float R2 = 22000.0;          // 分压电阻R2=22kΩ
const float REF_VOLTAGE = 3.29;    // ESP32 ADC参考电压（实测更准）
int voltage;                       //遥控电压
int voltage2;                      //车辆电压


//LED呼吸状态相关参数定义
unsigned long previousMillis = 0;   // 记录上次状态切换时间
const long onInterval = 1000;       // LED亮的时间
const long offInterval = 1000;      // LED灭的时间
bool isLedOn = false;               // LED当前状态（默认关闭）


//espnow配对相关参数
uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};   //广播地址
uint8_t slaveMac[6] = {0};                                           //从机MAC
volatile bool isPaired = false;                                      // 配对状态标志
volatile unsigned long lastResponseTime = 0;                         // 最后响应时间戳
const unsigned long RESPONSE_TIMEOUT = 1000;                          // 连接超时时间1S

// 定义配对消息
#define PAIRING_REQUEST "PAIRING_REQ"
#define PAIRING_RESPONSE "PAIRING_RESP"

// 初始化U8g2对象
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R2, 8, 9, 0x3C);

//开机logo
#define logo_width 120
#define logo_height 60
static const unsigned char logo_bits[] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0xF8, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0xF8, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0xFF, 0xFF, 0x0F, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0xFF, 0xFF, 0x3F, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 
  0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0xE0, 0x00, 0x00, 0xE0, 0xFF, 0xFF, 0xFF, 0xFF, 0x01, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0xC0, 0xFF, 0xFF, 0xFF, 
  0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x80, 
  0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 
  0x00, 0x00, 0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0xF0, 0xFF, 0xFF, 0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0xFF, 0xFF, 
  0xFF, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x03, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1C, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x80, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xFF, 0xFF, 0xFF, 
  0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x01, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x01, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0xF8, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x07, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 
  0x87, 0x0F, 0x3E, 0xF0, 0x81, 0x0F, 0x7C, 0xC6, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x70, 0xEC, 0x1F, 0xFF, 0xF8, 0xC3, 0x1F, 0xFF, 0xC6, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x6C, 0xB8, 0xC3, 0x1C, 0xE7, 0x38, 
  0xC3, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x3E, 0xB0, 0xC1, 
  0x0F, 0x6E, 0xB0, 0x83, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 
  0x37, 0xF0, 0x81, 0x0F, 0x3C, 0xF0, 0x81, 0x7D, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0xC0, 0x37, 0xF0, 0x81, 0x0F, 0x3C, 0xF0, 0x81, 0x1F, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xB0, 0xC1, 0x0F, 0x7E, 0xB0, 
  0x81, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7C, 0xB8, 0xC3, 
  0x1C, 0xE7, 0x38, 0xC3, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0xC0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  };

// 摇杆校准数据结构
struct {
  int LxMid, LyMid, RxMid, RyMid;
  int LxNegMax = 0, LxPosMax = 0;
  int LyNegMax = 0, LyPosMax = 0;
  int RxNegMax = 0, RxPosMax = 0;
  int RyNegMax = 0, RyPosMax = 0;
  // 比例系数缓存
  float LxNegRatio = 0, LxPosRatio = 0;
  float LyNegRatio = 0, LyPosRatio = 0;
  float RxNegRatio = 0, RxPosRatio = 0;
  float RyNegRatio = 0, RyPosRatio = 0;
} calibration;


// ESPNOW 控制数据包
typedef struct {
  int leftX;
  int leftY;
  int rightX;
  int rightY;
} ControlData;

ControlData controlData;

// ESPNOW 回传数据包
typedef struct Response {
  int responseValue;
} Response;

Response receivedResponse;

// 指数移动平均滤波
#define FILTER_COEFFICIENT 0.8
float filteredLX = 0;
float filteredLY = 0;
float filteredRX = 0;
float filteredRY = 0;

// 校准函数
int applyCalibration(float value, int mid) {
  int calibrated = value - mid;
  return abs(calibrated) < deadZone ? 0 : calibrated;
}

// 映射函数
int mapToDynamicRange(int value, float negRatio, float posRatio) {
  if(value == 0) return 0;
  
  if(value > 0) {
    return constrain(value * posRatio, 0, 255);
  } else {
    return constrain(value * negRatio, -255, 0);
  }
}


//定义摇杆滤波函数
void applyFilter(int rawLX, int rawLY, int rawRX, int rawRY) {
  filteredLX = FILTER_COEFFICIENT*rawLX + (1-FILTER_COEFFICIENT)*filteredLX;
  filteredLY = FILTER_COEFFICIENT*rawLY + (1-FILTER_COEFFICIENT)*filteredLY;
  filteredRX = FILTER_COEFFICIENT*rawRX + (1-FILTER_COEFFICIENT)*filteredRX;
  filteredRY = FILTER_COEFFICIENT*rawRY + (1-FILTER_COEFFICIENT)*filteredRY;
}

// 自动校准
void autoCalibrate(int samples = 500) {
  Serial.println("Calibrating... Keep joystick centered!");
  long sumLX = 0, sumLY = 0, sumRX = 0, sumRY = 0;
  int tmpLXmin = 4095, tmpLXmax = 0;
  int tmpLYmin = 4095, tmpLYmax = 0;
  int tmpRXmin = 4095, tmpRXmax = 0;
  int tmpRYmin = 4095, tmpRYmax = 0;

  for(int i=0; i<samples; i++){
    int Lx = analogRead(LEFT_X_PIN);
    int Ly = analogRead(LEFT_Y_PIN);
    int Rx = analogRead(RIGHT_X_PIN);
    int Ry = analogRead(RIGHT_Y_PIN);
    Ry = -Ry;
    Lx = -Lx;
    Rx = -Rx;
    sumLX += Lx;
    sumLY += Ly;
    sumRX += Rx;
    sumRY += Ry;
    tmpLXmin = min(tmpLXmin, Lx);
    tmpLXmax = max(tmpLXmax, Lx);
    tmpLYmin = min(tmpLYmin, Ly);
    tmpLYmax = max(tmpLYmax, Ly);
    tmpRXmin = min(tmpRXmin, Rx);
    tmpRXmax = max(tmpRXmax, Rx);
    tmpRYmin = min(tmpRYmin, Ry);
    tmpRYmax = max(tmpRYmax, Ry);
    delay(10);
  }

  calibration.LxMid = sumLX / samples;
  calibration.LyMid = sumLY / samples;
  calibration.RxMid = sumRX / samples;
  calibration.RyMid = sumRY / samples;

  // 计算初始动态范围
  calibration.LxNegMax = calibration.LxMid - tmpLXmin;
  calibration.LxPosMax = tmpLXmax - calibration.LxMid;
  calibration.LyNegMax = calibration.LyMid - tmpLYmin;
  calibration.LyPosMax = tmpLYmax - calibration.LyMid;
  calibration.RxNegMax = calibration.RxMid - tmpRXmin;
  calibration.RxPosMax = tmpRXmax - calibration.RxMid;
  calibration.RyNegMax = calibration.RyMid - tmpRYmin;
  calibration.RyPosMax = tmpRYmax - calibration.RyMid;

  // 预计算比例系数
  calibration.LxNegRatio = (calibration.LxNegMax > 0) ? 255.0 / calibration.LxNegMax : 0;
  calibration.LxPosRatio = (calibration.LxPosMax > 0) ? 255.0 / calibration.LxPosMax : 0;
  calibration.LyNegRatio = (calibration.LyNegMax > 0) ? 255.0 / calibration.LyNegMax : 0;
  calibration.LyPosRatio = (calibration.LyPosMax > 0) ? 255.0 / calibration.LyPosMax : 0;
  calibration.RxNegRatio = (calibration.RxNegMax > 0) ? 255.0 / calibration.RxNegMax : 0;
  calibration.RxPosRatio = (calibration.RxPosMax > 0) ? 255.0 / calibration.RxPosMax : 0;
  calibration.RyNegRatio = (calibration.RyNegMax > 0) ? 255.0 / calibration.RyNegMax : 0;
  calibration.RyPosRatio = (calibration.RyPosMax > 0) ? 255.0 / calibration.RyPosMax : 0;

}

// 动态范围更新
void updateDynamicRange() {
  int offsetLX = filteredLX - calibration.LxMid;
  int offsetLY = filteredLY - calibration.LyMid;
  int offsetRX = filteredRX - calibration.RxMid;
  int offsetRY = filteredRY - calibration.RyMid;

  // 更新LX轴范围及比例
  if(offsetLX < 0) {
    int absOffset = abs(offsetLX);
    if(absOffset > calibration.LxNegMax) {
      calibration.LxNegMax = absOffset;
      calibration.LxNegRatio = 255.0 / calibration.LxNegMax;
    }
  } else {
    if(offsetLX > calibration.LxPosMax) {
      calibration.LxPosMax = offsetLX;
      calibration.LxPosRatio = 255.0 / calibration.LxPosMax;
    }
  }

  // 更新RX轴范围及比例
  if(offsetRX < 0) {
    int absOffset = abs(offsetRX);
    if(absOffset > calibration.RxNegMax) {
      calibration.RxNegMax = absOffset;
      calibration.RxNegRatio = 255.0 / calibration.RxNegMax;
    }
  } else {
    if(offsetRX > calibration.RxPosMax) {
      calibration.RxPosMax = offsetRX;
      calibration.RxPosRatio = 255.0 / calibration.RxPosMax;
    }
  }

  // 更新LY轴范围及比例
  if(offsetLY < 0) {
    int absOffset = abs(offsetLY);
    if(absOffset > calibration.LyNegMax) {
      calibration.LyNegMax = absOffset;
      calibration.LyNegRatio = 255.0 / calibration.LyNegMax;
    }
  } else {
    if(offsetLY > calibration.LyPosMax) {
      calibration.LyPosMax = offsetLY;
      calibration.LyPosRatio = 255.0 / calibration.LyPosMax;
    }
  }

  // 更新RY轴范围及比例
  if(offsetRY < 0) {
    int absOffset = abs(offsetRY);
    if(absOffset > calibration.RyNegMax) {
      calibration.RyNegMax = absOffset;
      calibration.RyNegRatio = 255.0 / calibration.RyNegMax;
    }
  } else {
    if(offsetRY > calibration.RyPosMax) {
      calibration.RyPosMax = offsetRY;
      calibration.RyPosRatio = 255.0 / calibration.RyPosMax;
    }
  }

}


//测量电压函数
void DisplayAdc() {
  // 读取ADC原始值（取10次平均）
  int adcValue = 0;
  for(int i=0; i<10; i++) {
    adcValue += analogRead(ADC_PIN);
    delay(2);
  }
  adcValue /= 10;
  // 计算实际电压比例
  voltage = ((adcValue * REF_VOLTAGE / 4095.0) * ((R1 + R2)/R2))*10;
  if (voltage < 28){
    voltage = 0;
  }
  else if(voltage > 41){
    voltage = 100;
  }
  else {
    voltage = map(voltage, 28, 41, 0, 100);
  }
  delay(10);
}


// 发送回调
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("Last Packet Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
}

// 接收回调
void OnDataRecv(const esp_now_recv_info_t *recvInfo, const uint8_t *data, int len) {
  char msg[len + 1];
  memcpy(msg, data, len);
  memcpy(&receivedResponse, data, sizeof(receivedResponse));
  msg[len] = '\0';

  if (strcmp(msg, PAIRING_RESPONSE) == 0) {
    Serial.println("Pairing Response Received");
    memcpy(slaveMac, recvInfo->src_addr, 6); 
    
    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, slaveMac, 6);
    peerInfo.channel = 1; 
    peerInfo.encrypt = false;
    
    if (esp_now_add_peer(&peerInfo) == ESP_OK) {
      isPaired = true;
    }
  }
  //接收回传
  else  {
    voltage2 = receivedResponse.responseValue;
    delay(10);
  }
  //更新响应时间戳
  lastResponseTime = millis();
}


void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  WiFi.setChannel(1);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW); // 初始LED关闭

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW Init Failed");
    return;
  }

  // 初始化I2C引脚
  Wire.begin(11, 12); 
  // 初始化显示屏
  u8g2.begin();
  u8g2.setContrast(255);  // 设置对比度（0-255）
  u8g2.setFontPosTop();   //垂直位置设置为顶部
  u8g2.enableUTF8Print();  //设置支持中文字体
  u8g2.setFont(u8g2_font_wqy14_t_gb2312b);  //中文字体字号14
  u8g2.clearBuffer();   //  清空缓冲器
  u8g2.drawXBM(4, 1, logo_width, logo_height, logo_bits);   //显示开机logo
  u8g2.sendBuffer();  // 将缓冲区内容发送到屏幕
  delay(10);   // 延时

  autoCalibrate();  // 上电自动校准

  esp_now_register_send_cb(OnDataSent); //注册发送回调函数
  esp_now_register_recv_cb(OnDataRecv);  //注册接收回调函数

  // 添加广播对端（信道统一为1）
  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 1;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    return;
  }

  // 初始发送配对请求
  esp_now_send(broadcastAddress, (uint8_t *)PAIRING_REQUEST, strlen(PAIRING_REQUEST));
//  Serial.println("Pairing Request Sent");

}

void loop() {
  //读取摇杆数值
  int rawLX = analogRead(LEFT_X_PIN);
  int rawLY = analogRead(LEFT_Y_PIN);
  int rawRX = analogRead(RIGHT_X_PIN);
  int rawRY = analogRead(RIGHT_Y_PIN);
  rawRY = -rawRY;
  rawLX = -rawLX;
  rawRX = -rawRX;
  //滤波
  applyFilter(rawLX, rawLY, rawRX, rawRY);
  //动态范围更新
  updateDynamicRange();
  //校准
  int calLX = applyCalibration(filteredLX, calibration.LxMid);
  int calLY = applyCalibration(filteredLY, calibration.LyMid);
  int calRX = applyCalibration(filteredRX, calibration.RxMid);
  int calRY = applyCalibration(filteredRY, calibration.RyMid);
  // 使用预存比例进行映射
  int mappedLX = mapToDynamicRange(calLX, calibration.LxNegRatio, calibration.LxPosRatio);
  int mappedLY = mapToDynamicRange(calLY, calibration.LyNegRatio, calibration.LyPosRatio);
  int mappedRX = mapToDynamicRange(calRX, calibration.RxNegRatio, calibration.RxPosRatio);
  int mappedRY = mapToDynamicRange(calRY, calibration.RyNegRatio, calibration.RyPosRatio);

  delay(10);
  //摇杆数值写入控制数据包
  controlData.leftX = mappedLX;  
  controlData.leftY = mappedLY;
  controlData.rightX = mappedRX;  
  controlData.rightY = mappedRY;

  //连接状态发送控制数据包
  if (isPaired) {
    esp_now_send(slaveMac, (uint8_t *) &controlData, sizeof(controlData));
    //判断连接状态（指定时间未收到回传数据则认为失连）
    if (millis() - lastResponseTime > RESPONSE_TIMEOUT) {
      esp_now_del_peer(slaveMac); // 清除从机地址
      esp_now_register_send_cb(OnDataSent); //注册发送回调函数
      esp_now_register_recv_cb(OnDataRecv);  //注册接收回调函数
      isPaired = false;  //状态未未连接
     } 
    delay(10);
  }
  //未连接时进行广播配对
  else  {
    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
    peerInfo.channel = 1;
    peerInfo.encrypt = false;
    esp_now_send(broadcastAddress, (uint8_t *)PAIRING_REQUEST, strlen(PAIRING_REQUEST));
    voltage2 = 0;   //回传电压置0
  }

  //读取当前时间
  unsigned long currentMillis = millis();
  // 根据当前状态选择时间阈值
  long targetInterval = isLedOn ? onInterval : offInterval;
  //读取电压
  DisplayAdc();
  //显示屏操作
  u8g2.clearBuffer();  // 清空缓冲区
//显示第一行
  if (isPaired) {
    u8g2.setCursor(45, 5);
    u8g2.print("已连接");
    digitalWrite(LED_PIN, HIGH);
  } else {
    u8g2.setCursor(45, 5);
    u8g2.print("未连接");
    //未连接时LED呼吸状态
    if (currentMillis - previousMillis >= targetInterval) {
      previousMillis = currentMillis; // 重置计时器
      isLedOn = !isLedOn;             // 反转状态
      digitalWrite(LED_PIN, isLedOn ? HIGH : LOW); // 更新LED
    }
  }
  //显示遥控器电压
  u8g2.setCursor(10, 5);
  u8g2.print(voltage);
  //显示接收机回传电压
  u8g2.setCursor(105, 5);
  u8g2.print(voltage2);

//显示第二行
  u8g2.setCursor(10, 25);
  u8g2.print("LX: ");
  u8g2.print(mappedLX);
//  u8g2.print(analogRead(LEFT_Y_PIN));

  u8g2.setCursor(70, 25);
  u8g2.print("RX: ");
  u8g2.print(mappedRX);
//  u8g2.print(analogRead(RIGHT_Y_PIN));


//显示第三行
  u8g2.setCursor(10, 45);
  u8g2.print("LY: ");
  u8g2.print(mappedLY);
//  u8g2.print(analogRead(LEFT_Y_PIN));

  u8g2.setCursor(70, 45);
  u8g2.print("RY: ");
  u8g2.print(mappedRY);
//  u8g2.print(analogRead(RIGHT_Y_PIN));

  u8g2.sendBuffer();  // 将缓冲区内容发送到屏幕

}